/*----------------------------------------------------------------------
  - File name      : pid.c
  - Author         : Liu Chao
  - Update date    : 2020-07-31                   
  -	Copyright      : Gevico Electronic studio   
  - Module comments: 
-----------------------------------------------------------------------*/
/*-----------------------------------------------------------------------
|                               INCLUDES                                |
-----------------------------------------------------------------------*/
#include "pid.h"
#include "pwm.h"
#include "gui.h"
#include "color.h"
/*-----------------------------------------------------------------------
|                                 DATA                                  |
-----------------------------------------------------------------------*/
PID CTB_PIDX; //Control the ball of PID X-axis
uint8_t PWM_Max,PWM_Min;
extern RESULT Resu;
/*-----------------------------------------------------------------------
|                               FUNCTION                                |
-----------------------------------------------------------------------*/

/*--------------------------------------------------------
| @Description: PID parameters are initialized function  |
| @param      : None                                     |
| @return     : None                                     |
--------------------------------------------------------*/

void PID_Init(void)
{
  CTB_PIDX.Uv = 65.0; //User settings of X-axis value 
  CTB_PIDX.Kp = - 0.54; //User settings scale factor of X-axis value 
  CTB_PIDX.Outx = 15; 
  CTB_PIDX.ST = 20;  //The sampling time   uint: ms
  CTB_PIDX.Ti = 98.5; //The integral time   uint: ms
  CTB_PIDX.Td = 1.1; //The derivative time uint: ms
  PWM_Max = 25;
  PWM_Min = 5;

}  

/*--------------------------------------------------------
| @Description: Ball control task selection function     |
| @param      : PID CTB_PIDX,PID CTB_PIDY                |
| @return     : None                                     |
--------------------------------------------------------*/

void pid_task_handler(void)
{
	if(Resu.x != 0 && Resu.y != 0)
	{
		PID_Calc((double)Resu.x,(double)Resu.y); //Calculate PID parameters.             
	}
	else
	{
		PWM_Out(15);    
		#if 0		
		LCD_ShowString(2,110,12,"Cx:",1);
		LCD_ShowNum(22,110,90,3,12);
		#endif
	}
}

/*--------------------------------------------------------
| @Description: PID parameters calculation function      |
| @param      : None                                     |
| @return     : None                                     |
--------------------------------------------------------*/

 void PID_Calc(float SXv,float SYv)
 {
	 
  double Iout_X;
  double Dout_X;
  double Rout_X;
  #if 0
  double Anglex;
	#endif
	 
  CTB_PIDX.Sv = SXv;
  CTB_PIDX.Ek_2 = CTB_PIDX.Ek_1;  //Get last deviation of X-axis value 
  CTB_PIDX.Ek_1 = CTB_PIDX.Ek;  //Get last deviation of X-axis value 
  CTB_PIDX.Ek = CTB_PIDX.Uv - CTB_PIDX.Sv;  //Get current deviation of X-axis value 
  Iout_X = (CTB_PIDX.ST / CTB_PIDX.Ti) * CTB_PIDX.Ek;  //Get integral output of X-axis value 
  Dout_X = (CTB_PIDX.Td / CTB_PIDX.ST) * (CTB_PIDX.Ek - 2 * CTB_PIDX.Ek_1 + CTB_PIDX.Ek_2);  //Get derivative output of X-axis value 
  Rout_X = CTB_PIDX.Kp * (CTB_PIDX.Ek - CTB_PIDX.Ek_1 + Iout_X + Dout_X) + CTB_PIDX.Outx; //Get output results of X-axis value 

  if (Rout_X < PWM_Min)      CTB_PIDX.Out = PWM_Min;   //Set minimum value
  else if (Rout_X > PWM_Max) CTB_PIDX.Out = PWM_Max;   //Set maximum value
  else                       CTB_PIDX.Out =  Rout_X;
         
  PWM_Out(CTB_PIDX.Out);
	 
	#if 0	
	 
  Anglex = 9 * CTB_PIDX.Out - 45; //Get PWM to anglex of X-axis value 


  LCD_ShowString(2,110,12,"Cx:",1);
  LCD_ShowNum(22,110,Anglex,3,12);
	#endif
	
 }




/*-------END OF FLIE--------(C) COPYRIGHT Gevico Electronics-----------*/
